/*
 * Copyright (c) 2022 HiSilicon (Shanghai) Technologies CO., LIMITED.
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include <fcntl.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <signal.h>
#include <semaphore.h>
#include <pthread.h>

#include "sample_comm_ive.h"
#include "face_align.h"


#define FACE_ALIGN_IVE_PSP_WIDTH_250         250
#define FACE_ALIGN_IVE_PSP_HEIGHT_250        250
#define FACE_ALIGN_IVE_PSP_WIDTH_96          96
#define FACE_ALIGN_IVE_PSP_HEIGHT_112        112
#define FACE_ALIGN_IVE_PSP_POINT_PAIR_NUM_68 68
#define FACE_ALIGN_IVE_PSP_POINT_PAIR_NUM_5  5
#define FACE_ALIGN_IVE_PSP_POINT_FIX_BIT     2
#define FACE_ALIGN_IVE_PSP_IDX_1             1
#define FACE_ALIGN_IVE_PSP_IDX_2             2
#define FACE_ALIGN_IVE_PSP_IDX_3             3
#define FACE_ALIGN_IVE_PSP_LAND_MASK_OFFSET  4
#define FACE_ALIGN_IVE_PSP_MAX_ROI           64
#define FACE_ALIGN_IVE_PSP_MAX_POINT_PAIR    64


typedef struct hiFACE_ALIGN_IVE_PERSP_TRANS_S {
    //IVE_SRC_IMAGE_S stSrc;
    IVE_RECT_U32_S astRoi[FACE_ALIGN_IVE_PSP_MAX_ROI];
    HI_U16 u16RoiNum;
    //IVE_DST_IMAGE_S astDst[FACE_ALIGN_IVE_PSP_MAX_ROI];
    IVE_SRC_MEM_INFO_S astPointPair[FACE_ALIGN_IVE_PSP_MAX_POINT_PAIR];
    IVE_PERSP_TRANS_CTRL_S stPerspTransCtrl;

    // FILE *pFpSrc;
    // FILE *pFpDst;
} FACE_ALIGN_IVE_PERSP_TRANS_S;

static FACE_ALIGN_IVE_PERSP_TRANS_S s_stPerspTrans;
static HI_BOOL s_bStopSignal = HI_FALSE;

static HI_VOID FACE_ALIGN_IVE_PerspTrans_Uninit(FACE_ALIGN_IVE_PERSP_TRANS_S *pstPerspTrans)
{
    HI_U16 i;

   //IVE_MMZ_FREE(pstPerspTrans->stSrc.au64PhyAddr[0], pstPerspTrans->stSrc.au64VirAddr[0]);

    for (i = 0; i < pstPerspTrans->u16RoiNum; i++) {
   //     IVE_MMZ_FREE(pstPerspTrans->astDst[i].au64PhyAddr[0], pstPerspTrans->astDst[i].au64VirAddr[0]);
        IVE_MMZ_FREE(pstPerspTrans->astPointPair[i].u64PhyAddr, pstPerspTrans->astPointPair[i].u64VirAddr);
    }
}


static HI_S32 FACE_ALIGN_IVE_PerspTrans_Init(FACE_ALIGN_IVE_PERSP_TRANS_S *pstPerspTrans,HI_U32 u32DstWidth, HI_U32 u32DstHeight, IVE_RECT_U32_S astRoi[], HI_U16 u16RoiNum,
    HI_U16 u16MaxPointPairNum,const LandMark *orginPts)
{   
    LandMark pts = *orginPts;
    HI_S32 s32Ret = HI_SUCCESS;
    HI_U32 u32Size;
    HI_U16 i, j;
    HI_U16 au16LandMark[] = {   107, 109, 38, 52,
                                149, 117, 74, 52,
                                123, 135, 56, 72,
                                99,  157, 42, 92,
                                144, 157, 71, 92
                        };
    IVE_PERSP_TRANS_POINT_PAIR_S *pstTmp = NULL;

    au16LandMark[0] = pts.Eye_L.x;
    au16LandMark[1] = pts.Eye_L.y;
    au16LandMark[4] = pts.Eye_R.x;
    au16LandMark[5] = pts.Eye_R.y;
    au16LandMark[8] = pts.Nose.x;
    au16LandMark[9] =  pts.Nose.y;  
    au16LandMark[12] = pts.Mouth_L.x;
    au16LandMark[13] = pts.Mouth_L.y;
    au16LandMark[16] = pts.Mouth_R.x;
    au16LandMark[17] = pts.Mouth_R.y;
    /*****************打印变换后的人脸关键点**********************/
    // for(int i = 0; i<20 ;i++)
    // {
    //     printf("au16LandMark[%d] = %d\n",i,au16LandMark[i]);
    // }
    s32Ret =
        memcpy_s(pstPerspTrans->astRoi, sizeof(IVE_RECT_U32_S) * u16RoiNum, astRoi, sizeof(IVE_RECT_U32_S) * u16RoiNum);
    SAMPLE_CHECK_EXPR_RET(s32Ret != EOK, HI_ERR_IVE_ILLEGAL_PARAM, "memcpy_s failed!\n");


    for (i = 0; i < u16RoiNum; i++) {
        // s32Ret = SAMPLE_COMM_IVE_CreateImage(&(pstPerspTrans->astDst[i]), IVE_IMAGE_TYPE_YUV420SP, u32DstWidth,
        //     u32DstHeight);
        SAMPLE_CHECK_EXPR_GOTO(s32Ret != HI_SUCCESS, PERSP_TRANS_INIT_FAIL, "Error(%#x),Create src image failed!\n",
            s32Ret);
    }

    u32Size = sizeof(IVE_PERSP_TRANS_POINT_PAIR_S) * u16MaxPointPairNum;
    for (i = 0; i < u16RoiNum; i++) {
        s32Ret = SAMPLE_COMM_IVE_CreateMemInfo(&(pstPerspTrans->astPointPair[i]), u32Size);
        SAMPLE_CHECK_EXPR_GOTO(s32Ret != HI_SUCCESS, PERSP_TRANS_INIT_FAIL, "Error(%#x),Create src image failed!\n",
            s32Ret);
    }

    pstPerspTrans->stPerspTransCtrl.enAlgMode = IVE_PERSP_TRANS_ALG_MODE_AFFINE;	//仿射变换
    pstPerspTrans->stPerspTransCtrl.enCscMode = IVE_PERSP_TRANS_CSC_MODE_NONE;		//不做色彩空间转换
    pstPerspTrans->stPerspTransCtrl.u16RoiNum = u16RoiNum;
    pstPerspTrans->stPerspTransCtrl.u16PointPairNum = FACE_ALIGN_IVE_PSP_POINT_PAIR_NUM_5; /* point pair num: 5 */		//透视变换的点对信息数
    pstPerspTrans->u16RoiNum = u16RoiNum;

    for (i = 0; i < u16RoiNum; i++) {
        pstTmp = (IVE_PERSP_TRANS_POINT_PAIR_S *)(HI_UINTPTR_T)pstPerspTrans->astPointPair[i].u64VirAddr;
        for (j = 0; j < pstPerspTrans->stPerspTransCtrl.u16PointPairNum; j++) {
            /* x: au16LandMark[j * 4] << 2 */
            pstTmp->stSrcPoint.u14q2X = au16LandMark[j * FACE_ALIGN_IVE_PSP_LAND_MASK_OFFSET] <<
                FACE_ALIGN_IVE_PSP_POINT_FIX_BIT;
            /* y: au16LandMark[j * 4 + 1] << 2 */
            pstTmp->stSrcPoint.u14q2Y = au16LandMark[j * FACE_ALIGN_IVE_PSP_LAND_MASK_OFFSET + FACE_ALIGN_IVE_PSP_IDX_1] <<
                FACE_ALIGN_IVE_PSP_POINT_FIX_BIT;
            /* x:  au16LandMark[j * 4 + 2] << 2 */
            pstTmp->stDstPoint.u14q2X = au16LandMark[j * FACE_ALIGN_IVE_PSP_LAND_MASK_OFFSET + FACE_ALIGN_IVE_PSP_IDX_2] <<
                FACE_ALIGN_IVE_PSP_POINT_FIX_BIT;
            /* y: au16LandMark[j * 4 + 3] << 2 */
            pstTmp->stDstPoint.u14q2Y = au16LandMark[j * FACE_ALIGN_IVE_PSP_LAND_MASK_OFFSET + FACE_ALIGN_IVE_PSP_IDX_3] <<
                FACE_ALIGN_IVE_PSP_POINT_FIX_BIT;

            pstTmp++;
        }
    }

    return s32Ret;

PERSP_TRANS_INIT_FAIL:
    FACE_ALIGN_IVE_PerspTrans_Uninit(pstPerspTrans);

    return s32Ret;
}

static HI_S32 FACE_ALIGN_IVE_PerspTransProc(FACE_ALIGN_IVE_PERSP_TRANS_S *pstPerspTrans,IVE_IMAGE_S *src, IVE_IMAGE_S *dst)
{
    HI_S32 s32Ret = HI_FAILURE;
    IVE_HANDLE IveHandle;
    HI_BOOL bFinish = HI_FALSE;
    HI_BOOL bBlock = HI_TRUE;
    HI_BOOL bInstant = HI_TRUE;

    s32Ret = HI_MPI_IVE_PerspTrans(&IveHandle, src, pstPerspTrans->astRoi,
        pstPerspTrans->astPointPair, dst, &pstPerspTrans->stPerspTransCtrl, bInstant);
    SAMPLE_CHECK_EXPR_RET(s32Ret != HI_SUCCESS, s32Ret, "Error(%#x),HI_MPI_IVE_PerspTrans failed!\n", s32Ret);

    s32Ret = HI_MPI_IVE_Query(IveHandle, &bFinish, bBlock);
    while (s32Ret == HI_ERR_IVE_QUERY_TIMEOUT) {
        usleep(IVE_QUERY_SLEEP_TIME);
        s32Ret = HI_MPI_IVE_Query(IveHandle, &bFinish, bBlock);
    }
    SAMPLE_CHECK_EXPR_RET(s32Ret != HI_SUCCESS, s32Ret, "Error(%#x),HI_MPI_IVE_Query failed!\n", s32Ret);

/**********不保存图片的话可删*********************/
    /* 保存变换后的图片 */
    // s32Ret = SAMPLE_COMM_IVE_WriteFile(&pstPerspTrans->astDst[0], pstPerspTrans->pFpDst);
    // SAMPLE_CHECK_EXPR_RET(s32Ret != HI_SUCCESS, s32Ret, "Error(%#x),Read src file failed!\n", s32Ret);
/**********不保存图片的话可删*********************/
    return s32Ret;
}

static HI_VOID FACE_ALIGN_IVE_PerspTransStop(HI_VOID)
{
    FACE_ALIGN_IVE_PerspTrans_Uninit(&s_stPerspTrans);
    (HI_VOID)memset_s(&s_stPerspTrans, sizeof(s_stPerspTrans), 0, sizeof(s_stPerspTrans));
    SAMPLE_COMM_IVE_IveMpiExit();
    printf("\033[0;31mprogram termination abnormally!\033[0;39m\n");
}


HI_VOID FACE_ALIGN_IVE_PerspTrans(IVE_IMAGE_S *src, IVE_IMAGE_S *dst, HI_U32 DstWidth, HI_U32 DstHeight,const LandMark *orginPts)
{
    
    LandMark pts = *orginPts;
    HI_S32 s32Ret;
    HI_U32 u32SrcWidth =  src->u32Width;
    HI_U32 u32SrcHeight = src->u32Height;
    HI_U32 u32DstWidth =  DstWidth;
    HI_U32 u32DstHeight = DstHeight;
    //HI_CHAR achDstFileName[PATH_MAX] = {0};
   // HI_CHAR achSrcFileName[PATH_MAX] = {0};
    IVE_RECT_U32_S astRoi[2] = {0};
    HI_U16 u16RoiNum = 1;
    HI_U16 u16MaxPointPairNum = FACE_ALIGN_IVE_PSP_POINT_PAIR_NUM_68;
    s_bStopSignal = HI_FALSE;

    (HI_VOID)memset_s(&s_stPerspTrans, sizeof(s_stPerspTrans), 0, sizeof(s_stPerspTrans));
    //SAMPLE_COMM_IVE_CheckIveMpiInit();
 
    // printf("imgDst.Leye = (%d,%d), imgDst.Reye = (%d,%d)\n", pts.Eye_L.x,
    // pts.Eye_L.y, pts.Eye_R.x, pts.Eye_R.y);

    // printf("imgDst.Nose = (%d,%d)\n", pts.Nose.x,pts.Nose.y);
    
    // printf("imgDst.Mouth_L = (%d,%d), imgDst.Mouth_R = (%d,%d)\n", pts.Mouth_L.x,
    // pts.Mouth_L.y, pts.Mouth_R.x, pts.Mouth_R.y);
/**********不保存图片的话可删*********************/
    //设置保存路径及保存文件
    // if (snprintf_s(achSrcFileName, sizeof(achSrcFileName), sizeof(achSrcFileName) - 1,
    //     "./data/output/psp/complete_%s.yuv", "crop2") < 0) {
    //     HI_ASSERT(0);
    //     }
    // printf("Src.h = %d, Src.w = %d\n", s_stPerspTrans.stSrc.u32Height,s_stPerspTrans.stSrc.u32Width);
    // s_stPerspTrans.pFpSrc = fopen(achSrcFileName, "wb");
    // SAMPLE_COMM_IVE_WriteFile(&s_stPerspTrans.stSrc, s_stPerspTrans.pFpSrc); // write yuv file

    // if (snprintf_s(achDstFileName, sizeof(achDstFileName), sizeof(achDstFileName) - 1,
    //     "./data/output/psp/complete_%s.yuv", "psp") < 0) {
    //     HI_ASSERT(0);
    //     }
    
    // s_stPerspTrans.pFpDst = fopen(achDstFileName, "wb");
    // SAMPLE_COMM_IVE_WriteFile(&s_stPerspTrans.pFpDst, s_stPerspTrans.pFpDst); // write yuv file
/**********不保存图片的话可删*********************/

    astRoi[0].u32X = 0;
    astRoi[0].u32Y = 0;
    astRoi[0].u32Width = u32SrcWidth;
    astRoi[0].u32Height = u32SrcHeight;
    s32Ret = FACE_ALIGN_IVE_PerspTrans_Init(&s_stPerspTrans, u32DstWidth, u32DstHeight, astRoi,
            u16RoiNum, u16MaxPointPairNum, &pts);
    // SAMPLE_CHECK_EXPR_GOTO(s32Ret != HI_SUCCESS, PERSP_TRANS_FAIL, "Error(%#x),FACE_ALIGNE_PerspTrans_Init failed!\n",
    //     s32Ret);

    if (s_bStopSignal == HI_TRUE) {
        FACE_ALIGN_IVE_PerspTransStop();
        return;
    }

    s32Ret = FACE_ALIGN_IVE_PerspTransProc(&s_stPerspTrans,src,dst);
    if (s32Ret == HI_SUCCESS) {
        SAMPLE_PRT("Process success!\n");
    }

    if (s_bStopSignal == HI_TRUE) {
        FACE_ALIGN_IVE_PerspTransStop();
        return;
    }
    FACE_ALIGN_IVE_PerspTrans_Uninit(&s_stPerspTrans);   
    (HI_VOID)memset_s(&s_stPerspTrans, sizeof(s_stPerspTrans), 0, sizeof(s_stPerspTrans));

// PERSP_TRANS_FAIL:
//     SAMPLE_COMM_IVE_IveMpiExit();
    
    
}

/* function : PerspTrans sample signal handle */
HI_VOID FACE_ALIGN_IVE_PerspTrans_HandleSig(HI_VOID)
{
    s_bStopSignal = HI_TRUE;
}
